#include "pid.h"

pid_t motorA, motorB, angle;

void pid_init(pid_t *pid, uint32_t mode, float p, float i, float d)
{
    pid->pid_mode = mode;
	pid->p = p;
	pid->i = i;
	pid->d = d;
}

void pid_control()
{
    /*角度环*/
    // 1. 设置目标角度
    angle.target = 0;
    // 2. 获取当前角度
	angle.now = yaw_mapped - yaw_ref;
    // 3. PID 控制器计算输出
    pid_cal(&angle);
    
    /*速度环*/
    // 1.设定目标速度
	int base_speed = 120; // 设定基础速度
    motorA.target = base_speed;
    motorB.target = base_speed;
    // 2.获取当前速度
    motorB.now = Encoder_Count1;
    Encoder_Count1 = 0;
    
    motorA.now = Encoder_Count2;
    Encoder_Count2 = 0;
    // 3.输入PID控制器进行计算
    pid_cal(&motorA);
    pid_cal(&motorB);
	// 4.计算左右电机的最终输出（加入差值补偿）
	motorA.out += (angle.out * 3.0);
	motorB.out -= (angle.out * 3.0);
    // 5.电机输出限幅
    pidout_limit(&motorA);
    pidout_limit(&motorB);
    // 6.PID的输出值 输入给电机
	MotorLeft_SetSpeed(motorB.out);
    MotorRight_SetSpeed(motorA.out);
//    __HAL_TIM_SET_COMPARE(&htim2, TIM_CHANNEL_3, motorA.out / 500);
//    __HAL_TIM_SET_COMPARE(&htim2, TIM_CHANNEL_4, motorB.out / 500);
}

void pid_cal(pid_t *pid)
{
    // 计算当前误差
    pid->error[0] = pid->target - pid->now;

    // 计算输出
    if (pid->pid_mode == DELTA_PID) // 增量式
    {
        pid->pout = pid->p * (pid->error[0] - pid->error[1]);
        pid->iout = pid->i * pid->error[0];
        pid->dout = pid->d * (pid->error[0] - 2 * pid->error[1] + pid->error[2]);
        pid->out += pid->pout + pid->iout + pid->dout;
    }
    else if (pid->pid_mode == POSITION_PID) // 位置式
    {
        pid->pout = pid->p * pid->error[0];
        pid->iout += pid->i * pid->error[0];
        pid->dout = pid->d * (pid->error[0] - pid->error[1]);
        pid->out = pid->pout + pid->iout + pid->dout;
    }

    // 记录前两次误差
    pid->error[2] = pid->error[1];
    pid->error[1] = pid->error[0];
}

void pidout_limit(pid_t *pid)
{
	// 输出限幅
	if(pid->out>=50000)	
		pid->out=50000;
	if(pid->out<=0)	
		pid->out=0;
}

void motor_target_set(int spe1, int spe2)
{
    motorA.target = spe1;
    motorB.target = -spe2;
}

























